#include "servo_control.h"
#include "ch32v30x.h"

void SERVO_Init(void) {
    GPIO_InitTypeDef GPIO_InitStructure;
    TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
    TIM_OCInitTypeDef TIM_OCInitStructure;

    // 1. 启用时钟
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | SERVO_TIMER_CLK | RCC_APB2Periph_AFIO, ENABLE);

    // 2. 配置舵机信号引脚 (PA8 - TIM1_CH1)
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(GPIOA, &GPIO_InitStructure);

    // 3. 定时器基础配置
    TIM_TimeBaseStructure.TIM_Period = PWM_PERIOD_US - 1; // 20ms周期
    TIM_TimeBaseStructure.TIM_Prescaler = (SystemCoreClock / 1000000) - 1; // 1MHz计数频率
    TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimeBaseInit(SERVO_TIMER, &TIM_TimeBaseStructure);

    // 4. PWM通道配置
    TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    TIM_OCInitStructure.TIM_Pulse = DS3120_MIN_US; // 初始脉宽1ms
    TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
    TIM_OC1Init(SERVO_TIMER, &TIM_OCInitStructure);

    // 5. 启用功能
    TIM_OC1PreloadConfig(SERVO_TIMER, TIM_OCPreload_Enable);
    TIM_CtrlPWMOutputs(SERVO_TIMER, ENABLE); // 关键：高级定时器必须启用
    TIM_Cmd(SERVO_TIMER, ENABLE); // 启动定时器
}

void SERVO_SetAngle(float angle) {
    // 角度限幅
    if(angle < SERVO_MIN_ANGLE) angle = SERVO_MIN_ANGLE;
    if(angle > SERVO_MAX_ANGLE) angle = SERVO_MAX_ANGLE;

    // 计算脉宽（微秒）
    uint16_t pulse_width = DS3120_MIN_US +
                          (uint16_t)((angle * (DS3120_MAX_US - DS3120_MIN_US)) / SERVO_MAX_ANGLE);

    // 更新比较寄存器
    TIM_SetCompare1(SERVO_TIMER, pulse_width);
}
